/**
 * Base class for any algorithm. Contains members and functions that most algorithms can use as well as
 * the definition for commands returned by an algorithm
 */

using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;

namespace Autobot
{
    // LEARNING TIME:
    // An algorithm has three data structures to manipulate. 
    // ACTION: an enum with an identifier of what to do
    // COMMAND: A struct that contains an action and parameters that are used to describe 'by how much'
    // RESULT: Finally when the algorithm decides what to do, there might be more than one command to fulfill the decision.
    //          This structure contains multiple COMMANDS


    // Here are the actions that the algorithm may perform
    enum AlgorithmAction : int
    {
        NONE,               // No change in state
        STOP,               // Robot comes to a full stop
        CHANGE_DIRECTION,   // Change the direction of a motor
        INC_SPEED,          // Robot increases speed in the current direction
        DEC_SPEED,          // Robot decreases speed in the current direction
        TURN_LEFT,          // Robot turns wheels to the left of current position
        TURN_RIGHT,         // Robot turns wheels to the right of the current position
        PANIC,              // Robot enters panic mode
        SET_DRIVE_MOTOR,    // Set the drive motor to specific setting
        SET_STEER_MOTOR     // Set the steering motor to a specific setting
    }

    // The algorithm is going to come up with actions
    // so we need to know what they are and what their parameters
    // would be.
    struct AlgorithmCommand
    {
        public int Action;
        public int[] Params;
        public int NumParams;
    }
   
    // The result of an algorithm decision might contain multiple commands
    class AlgorithmResult
    {
        private List<AlgorithmCommand> Commands;
 
        public AlgorithmResult()
        {
            Commands = new List<AlgorithmCommand>();
        }
        public void AddCommand(AlgorithmCommand NewCommand)
        {
            Commands.Add(NewCommand);
        }
        public int GetNumCommands()
        {
            return Commands.Count;
        }
        public AlgorithmCommand GetCommand(int i)
        {
            if(i>=0 && i<Commands.Count)
                return Commands[i];
            else
            {
                throw(new Exception("Bad Algorithm Command Index"));
            }
        }
    }

    abstract class AlgorithmBase
    {
        // Static string to identify this class in the Algorithms class
        public static readonly string Name = "AlgorithmBase";

        // References to the map, robot, and destination should be stored in every algorithm's memory
        protected VirtualMap map;
        protected Robot robot;
        protected Goal goal;

        // List of previously given results, for comparison
        protected Stack<AlgorithmResult> previousResults = new Stack<AlgorithmResult>();

        // Call this method when asking the algorithm what to do next
        // Abstract base class by default is just stop
        public virtual AlgorithmResult GetNextResult()
        {
            return CreateStopResult();
        }

        /// <summary>
        /// The following functions are helper to easily fill out commands
        /// </summary>
        /// <returns></returns>
        protected AlgorithmCommand CreateStopCommand(int DelayMS)
        {
            AlgorithmCommand StopCommand = new AlgorithmCommand();
            StopCommand.Action = (int)AlgorithmAction.STOP;
            StopCommand.NumParams = 1;
            StopCommand.Params = new int[1];
            StopCommand.Params[0] = DelayMS;

            return StopCommand;
        }
        protected AlgorithmCommand CreateNoCommand()
        {
            AlgorithmCommand NoCommand = new AlgorithmCommand();
            NoCommand.Action = (int)AlgorithmAction.NONE;
            NoCommand.NumParams = 0;
            NoCommand.Params = null;
           
            return NoCommand;
        }
        protected AlgorithmCommand CreateChangeDirectionCommand(int WhichMotor, int DelayMS)
        {
            AlgorithmCommand ChangeDirCommand = new AlgorithmCommand();
            ChangeDirCommand.Action = (int)AlgorithmAction.CHANGE_DIRECTION;
            ChangeDirCommand.NumParams = 2;
            ChangeDirCommand.Params = new int[2];
            ChangeDirCommand.Params[0] = WhichMotor;
            ChangeDirCommand.Params[1] = DelayMS;

            return ChangeDirCommand;
        }
        protected AlgorithmCommand CreateIncSpeedCommand(int HowMuch, int DelayMS)
        {
            AlgorithmCommand IncSpeedCommand = new AlgorithmCommand();
            IncSpeedCommand.Action = (int)AlgorithmAction.INC_SPEED;
            IncSpeedCommand.NumParams = 2;
            IncSpeedCommand.Params = new int[2];
            IncSpeedCommand.Params[0] = HowMuch;
            IncSpeedCommand.Params[1] = DelayMS;

            return IncSpeedCommand;
        }
        protected AlgorithmCommand CreateDecSpeedCommand(int HowMuch, int DelayMS)
        {
            AlgorithmCommand DecSpeedCommand = new AlgorithmCommand();
            DecSpeedCommand.Action = (int)AlgorithmAction.DEC_SPEED;
            DecSpeedCommand.NumParams = 2;
            DecSpeedCommand.Params = new int[2];
            DecSpeedCommand.Params[0] = HowMuch;
            DecSpeedCommand.Params[1] = DelayMS;

            return DecSpeedCommand;
        }
        protected AlgorithmCommand CreateTurnLeftCommand(int HowMuch, int DelayMS)
        {
            AlgorithmCommand TurnLeftCommand = new AlgorithmCommand();
            TurnLeftCommand.Action = (int)AlgorithmAction.TURN_LEFT;
            TurnLeftCommand.NumParams = 2;
            TurnLeftCommand.Params = new int[2];
            TurnLeftCommand.Params[0] = HowMuch;
            TurnLeftCommand.Params[1] = DelayMS;

            return TurnLeftCommand;
        }
        protected AlgorithmCommand CreateTurnRightCommand(int HowMuch, int DelayMS)
        {
            AlgorithmCommand TurnRightCommand = new AlgorithmCommand();
            TurnRightCommand.Action = (int)AlgorithmAction.TURN_RIGHT;
            TurnRightCommand.NumParams = 2;
            TurnRightCommand.Params = new int[2];
            TurnRightCommand.Params[0] = HowMuch;
            TurnRightCommand.Params[1] = DelayMS;

            return TurnRightCommand;
        }
        protected AlgorithmCommand CreatePanicCommand(int DelayMS)
        {
            AlgorithmCommand PanicCommand = new AlgorithmCommand();
            PanicCommand.Action = (int)AlgorithmAction.PANIC;
            PanicCommand.NumParams = 1;
            PanicCommand.Params = new int[1];
            PanicCommand.Params[0] = DelayMS;

            return PanicCommand;
        }
        protected AlgorithmCommand CreateSetDriveMotorCommand(int Setting, int DelayMS)
        {
            AlgorithmCommand SetDriveCommand = new AlgorithmCommand();
            SetDriveCommand.Action = (int)AlgorithmAction.SET_DRIVE_MOTOR;
            SetDriveCommand.NumParams = 2;
            SetDriveCommand.Params = new int[2];
            SetDriveCommand.Params[0] = Setting;
            SetDriveCommand.Params[1] = DelayMS;

            return SetDriveCommand;   
        }
        protected AlgorithmCommand CreateSetSteerMotorCommand(int Setting, int DelayMS)
        {
            AlgorithmCommand SetSteerCommand = new AlgorithmCommand();
            SetSteerCommand.Action = (int)AlgorithmAction.SET_STEER_MOTOR;
            SetSteerCommand.NumParams = 2;
            SetSteerCommand.Params = new int[2];
            SetSteerCommand.Params[0] = Setting;
            SetSteerCommand.Params[1] = DelayMS;

            return SetSteerCommand;
        }
        
        /// The Following Function is to make a quick and easy stop result
        protected AlgorithmResult CreateStopResult()
        {
            AlgorithmResult Stop = new AlgorithmResult();
            Stop.AddCommand(CreateStopCommand(0));
            return Stop;
        }

        // Get the reverse command for a given command
        protected AlgorithmCommand ReverseCommand(AlgorithmCommand command)
        {
            AlgorithmCommand reverseCommand  = command;
            switch ((int)command.Action)
            {
                case((int)AlgorithmAction.INC_SPEED):
                    reverseCommand.Action = (int)AlgorithmAction.DEC_SPEED;
                    break;
                case((int)AlgorithmAction.DEC_SPEED):
                    reverseCommand.Action = (int)AlgorithmAction.INC_SPEED;
                    break;
                case((int)AlgorithmAction.TURN_LEFT):
                    reverseCommand.Action = (int)AlgorithmAction.TURN_RIGHT;
                    break;
                case ((int)AlgorithmAction.TURN_RIGHT):
                    reverseCommand.Action = (int)AlgorithmAction.TURN_LEFT;
                    break;
                
                default:
                    // Don't reverse it if you can't directly interpret it.
                    return command;

            }
            return reverseCommand;
        }
    }
}